# Test ur5 move and socket

# urx
import urx
import math3d as m3d
# stdlib
import math
from math import pi, sin, cos, atan2
import time 
import sys
import logging
import thread
import numpy
# socket
import websocket
import copy



class ur5_move_pose():

    def __init__(self):
        self.rob = urx.Robot("192.168.0.108")
        self.rob.set_tcp((-0.038890873, -0.038890873, 0.22, 0, 0, -math.pi/4.0))
        self.rob.set_payload(0.5, (0, 0, 0))

        self.shift = 0.20
        self.vel = 0.1
        self.acc = 0.5
        self.vel1 = 0.5
        self.acc1 = 1.0

        # Work parameter
        self.radius = 0.20
        # Work waypoint1
        self.wp1 = [-0.6345370825877593, -0.11370179594088668, 0.020112582589867527, -1.2095804942110995, -1.2094686502533067, -1.2090269243075351]
        # self.wp2 = [-0.5440058756793467, -0.3161720850897343, 0.18856298256517964, -2.0175830180350975, -1.3141503392198488, -0.4855594004998023]
        # self.wp3 = []



        # Initialize joints
        self.initJoints = [0.009891480207443237, -1.424952809010641, 1.3706169128417969, -1.5393026510821741, -1.577808682118551, 0.7439982891082764]
        
        # Safely start
        tmp_pose = self.rob.getl()
        tmp_length = math.sqrt(numpy.square(tmp_pose[0] -self.wp1[0]) + numpy.square(tmp_pose[1] -self.wp1[1]) + numpy.square(tmp_pose[2] -self.wp1[2]) )
        if tmp_length <= 0.10:
            tmp_pose[2] += 0.05
            tmp_pose[3]  = 2.221441431          #-math.pi
            tmp_pose[4]  = 2.221441431
            tmp_pose[5]  = 0.0         # -math.pi / 4.0
            self.rob.movel(tmp_pose, self.acc, self.vel, wait = True)
        
        self.rob.movej(self.initJoints, self.acc1, self.vel1, wait = True)

        print("Moving to wp1...")
        self.rob.movel(self.wp1, 0.5, self.vel1, wait = True)

        # self.ws = websocket.create_connection("ws://192.168.0.147:9001")
        # print("Create ws connection.")

        # Start capture
        self.ws_get_pose("capture")

        print('Initialize Done.')


    # type 0: getj(); 1: getl() 2: get_pose(); 
    def get_current_pose(self, type = 0):
        # Get joints
        if type == 0:
            return self.rob.getj()
        # Get pose (x, y, z, rx, ry, rz)
        elif type == 1:
            return self.rob.getl()
        # Get current transform from base to to tcp
        elif type == 2:
            return self.rob.get_pose()

    def vacuum_suck(self, status):
        self.rob.set_digital_out(0, status)

    def vacuum_blow(self, status):
        self.rob.set_digital_out(1, status)

    def set_tool_io(self, voltage = 0, state = False):
        self.rob.set_tool_voltage(voltage)
        self.rob.set_digital_out(8, state)

    def ws_get_pose(self, command):
        # self.ws = websocket.create_connection("ws://192.168.0.147:9001")
        # print("Create ws connection.")
        if command == "capture":
            # self.ws.send("{\" capture\"}")
            print("Start capture.")
        elif command == "get_pose":
            # self.ws.send("{\"get_pose\"}")
            # self.recv = self.ws.recv()
            print("ws received: " + "self.recv")

        # self.ws.send("{\"conten\":\"robot\"}")
        # self.recv = self.ws.recv()
        # print("ws received ", self.recv)
        # ws.close()

    def close_program(self):
        print("Stopping the robot!")
        # self.ws.close()
        # Blow
        self.vacuum_suck(False)
        # Move to init pose
        # self.rob.movej(self.initJoints, self.acc1, self.vel1, wait = True)
        # Stop robot
        # self.rob.stopj()
        self.rob.stop()
        # robot socket
        self.rob.close()

    # def parse_json(self, data):
        
        
    def send_pose(self):
        # Move to init joints
        # print("Moving to Initial joint...")
        # self.rob.movej(self.initJoints, self.acc, self.vel, wait = True)
        
        # Start work
        # wp1
        print("Moving to wp1...")
        self.rob.movel(self.wp1, self.acc, self.vel, wait = True)
        # wp2
        self.wp2 = copy.deepcopy(self.wp1)
        # Relative
        # self.wp2[1] += self.radius * 0.707106781
        # self.wp2[2] += self.radius * 0.707106781
        # Static (rx, ry, rz) = (0, 0, , -pi/4)
        self.wp2[3]  = 2.221441431          #-math.pi
        self.wp2[4]  = 2.221441431
        self.wp2[5]  = 0.0         # -math.pi / 4.0
        # print("wp2[3]: ", self.wp2[3], self.wp2[4], self.wp2[5])

        print("Moving circle to wp2...")
        # self.rob.movels([self.wp1, self.wp2], self.acc, self.vel, self.radius, wait = True)
        self.rob.movel(self.wp2, self.acc, self.vel, wait = True)
        # # wp3
        self.wp3 = copy.deepcopy(self.wp2)
        self.wp3[1] += self.shift
        print("Moving to wp3...")
        self.rob.movel(self.wp3, self.acc1, self.vel1, wait = True)

#         # wp4
#         self.wp4 = copy.deepcopy(self.wp3)
#         self.wp4[3] -= math.pi/4
#         print("Moving circle to wp4...")
#         self.rob.movels([self.wp3, self.wp4], self.acc, self.vel, wait = True)

# ##########----Back----########################
#         print("Moving back to wp3...")
#         self.rob.movel(self.wp3, self.acc, self.vel, wait = True)
        # wp2
        print("Moving back to wp2...")
        self.rob.movel(self.wp2, self.acc1, self.vel1, wait = True)


        

if __name__ == "__main__":
    logging.basicConfig(level=logging.INFO)

    ur5_move_pose = ur5_move_pose()

    try:
      
        while True:
            ur5_move_pose.send_pose()


    # except:
    #     ur5_move_pose.close_program()
    finally:
        ur5_move_pose.close_program()


